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1  Introduction 


This  document  is  the  final  report  for  the  grant  titled  “Research  in  Image-Based  Cooperation 
for  Autonomous  Conventional  Aerial  Vehicles” ,  issued  in  May  of  2005  by  the  Air  Force  Office 
of  Scientific  Research.  The  research  was  performed  during  the  period  May,  2005  through 
September,  2008. 

The  overall  goal  of  the  research  described  was  to  develop  a  structure  in  which  the  information 
from  a  vision  sensor  (generally,  a  CCD  camera)  can  be  incorporated  into  the  information 
stream  from  the  sensors  on  an  inexpensive  aerial  vehicle.  The  purpose  of  -this  is  to  enhance 
cooperative  activities  of  multiple  such  vehicles.  One  of  the  defining  characteristics  is  that 
these  are  conventional  airframes;  that  is,  they  are  fixed-wing  aircraft  that  generate  lift  in  the 
standard  manner. 

1.1  Areas  of  Research 

The  work  described  herein  is  preliminary  in  nature.  A  result  of  this  is  that  it  is  piecemeal. 
Three  avenues  of  research  were  undertaken,  with  the  goal  of  bringing  them  together  as  they 
reached  maturity.  In  the  three  years  of  the  effort,  solid  gains  were  made  in  each  area,  but 
none  was  sufficiently  advanced  to  begin  to  blend  with  the  others. 

A  final  point  to  be  made  is  that  in  each  area,  the  attempt  was  to  use  the  least  possible 
information  from  the  vision  sensor.  This  is  in  keeping  with  the  desire  to  maintain  a  simple  and 
easily-implemented  structure,  that  could  be  used  on  minimally-capable  hardware.  Further, 
in  each  case  the  information  use  was  passive;  that  is,  there  is  no  communication  required 
between  the  vehicles.  While  cooperative  work  with  information  sharing  has  been  shown  to 
be  very  effective[ll,  the  load  on  the  computational  and  communications  systems  is  too  great 
for  implementation  on  the  classes  of  vehicles  being  considered  here. 

The  three  areas  of  research  were 

1.  Vision-based  Tracking 

In  this  effort,  one  aircraft  follows  another,  using  information  only  from  the  vision  sensor 
for  guidance.  This  is  a  beginning  step  in  cooperative  tasks  such  as  cooperative  sorties. 

2.  Cooperative  Observation  of  Partially  Occluded  Ground  Targets 

One  of  the  primary  uses  of  unmanned  aerial  vehicles  will  be  the  observation  of  ground 
targets.  Most  (if  not  all)  of  the  existing  work  in  this  area  assumes  that  the  targets  are 
visible  from  all  angles.  In  this  effort,  that  assumption  was  not  made. 

The  first  step  here  was  to  find  an  orbit  for  a  single  vehicle  that  kept  the  target  in  sight. 
The  measure  of  merit  was  the  duration  of  the  orbit,  which  was  maximized.  The  second 
step,  only  touched  upon  before  the  end  of  the  effort,  was  to  do  the  problem  with  a  pair 
of  vehicles. 

3.  Robust  Control  and  On-line  Parameter  Estimation 
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The  use  of  vision  sensors  introduces  additional  information  into  the  state  estimation 
process.  This  additional  information  can  be  used  in  at  least  two  ways.  First,  the 
additional  information  will  produce  a  solution  closer  to  the  actual  states  when  blended 
into  the  standard  data  fusion  filter  generally  used  on  the  classes  of  small,  inexpensive 
UAVs  envisioned  in  this  report.  Second,  the  information  might  be  taken  as  explicitly 
redundant;  that  is,  it  might  be  used  as  a  check  on  the  output  of  the  fusion  filter. 

In  the  work  reported  here,  the  first  approach  was  used.  The  resulting  estimates  of  states 
were  then  used  as  the  bases  for  (a)  a  neural  net  based  approach  to  robust  control,  and 
(b)  a  scheme  to  estimate  on-line  the  physical  parameters  of  the  UAV. 

Each  of  these  areas  is  discussed  in  detail  below.  One  or  more  master  of  science  theses  were 
written  in  each  area.  Before  discussing  these  applications,  however,  the  inclusion  of  vision 
information  in  the  data  fusion  filter  is  addressed. 


1.2  Personnel  Supported 

In  addition  to  the  principal  investigator,  who  was  supported  for  between  zero  and  four  weeks 
during  each  of  the  three  summers  during  which  the  grant  was  active,  the  grant  supported  four 
masters-level  graduate  students.  These  were  Rajtilok  Chakravarty,  Jaime  Alba-Bohorquez, 
Joe  Bhaktiar,  and  Tatiana  Sviridova.  These  students  were  also  partially  supported  by  the 
MAE  department.  Cooperative  work  was  done  by  Dale  Turner,  another  master’s  student, 
without  charge  to  the  grant.  Each  of  these  five  students  wrote  a  thesis  on  the  topic  of 
research. 

At  the  time  of  this  writing,  only  one  paper  has  been  published  as  a  result  of  these  efforts. 
This  is  largely  due  to  the  timing;  much  of  the  work  has  only  recently  finished,  so  that  papers 
have  not  yet  been  written  and  submitted. 


2  Including  Vision  in  the  Data  Fusion  Filter 

The  basis  for  much  of  the  work  performed  under  this  grant  was  the  inclusion  of  vision 
information  into  the  data  fusion  filter  (the  exception  is  the  vision  based  tracking  section). 
Briefly,  we  recall  that  the  standard  instrumentation  found  on  the  class  of  Unmanned  Aerial 
Vehicles  in  question  includes  a  low-cost  inertial  measurement  system  (IMU)  and  a  Global 
Positioning  System  (GPS)  receiver.  The  outputs  from  these  devices  are  blended,  in  effect 
using  the  GPS  results  to  perform  on-line  calibrations  and  bias  estimates  for  the  IMU.  [2]  This 
allows  the  use  of  far  lighter  and  less  expensive  sensors  than  those  needed  in  the  past. 

In  the  past  decade,  vision  systems  (generally,  Charge-Coupled  Device  (CCD)  cameras)  have 
become  widely  available  and  increasingly  less  expensive.  These  have  been  incorporated  into 
many  current  UAV  designs.  It  is  well  known  that  it  is  theoretically  possible  to  estimate 
all  velocities  and  orientations  using  solely  the  vision  information;  this  is  the  Eight-point 
Theorem [3],  However,  this  requires  the  isolation  and  tracking  of  at  least  eight  feature  points 
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over  several  frames.  This  has  been  investigated,  and  despite  problems,  may  be  viable  in 
some  cases  [4]. 

When  additional  information  is  included  in  the  data  extraction  process,  however,  the  problem 
is  much  easier [5].  In  the  work  reported  here,  the  only  information  extracted  from  the  vision 
sensor  is  the  velocity  direction.  As  the  orientation  of  the  camera  is  known  relative  to  the 
UAV,  this  gives  the  velocity  direction  in  body  frame.  In  the  standard  fusion  filter,  the  only 
direct  velocity  information  is  from  the  GPS,  which  cannot  provide  information  in  the  body 
frame.  This  leads  to  a  lack  of  observability,  in  particular,  the  inability  to  accurately  estimate 
sideslip.  Using  the  direction  information  from  the  vision  sensor  eliminates  this  problem,  and 
renders  the  system  fully  observable. 

A  secondary  benefit  of  including  this  information  is  that  it  is  no  longer  necessary  to  have  a 
full  solution  from  the  GPS  receiver.  Instead,  it  is  sufficient  to  use  the  velocity  information 
from  three  satellites. 

The  results  of  the  research  show  that  the  addition  of  velocity  direction  provides  a  significant 
increase  in  the  accuracy  of  the  data  fusion  filter.  Further,  the  filter  maintains  a  useful 
estimate  even  when  information  from  only  three  GPS  satellites  is  available.  These  results 
are  reported  in  Chakravarty  and  Chichka[6],  which  is  included  as  Appendix  A. 


3  Vision-based  Tracking 

In  this  effort,  one  aircraft  follows  another,  using  information  only  from  the  vision  sensor  for 
guidance.  It  is  assumed  only  that  the  observing  aircraft  can  extract  the  silhouette  of  the 
craft  it  is  following  from  the  camera.  The  work  reported  here  predates  some  recent  advances 
in  the  field[7]. 


Top  View  Rear  View  Side  View 


Figure  1:  Silhouette  in  bounding  box. 

In  keeping  with  the  ideal  of  minimal  information,  the  tracking  is  performed  using  only  the 
boundaries  of  the  craft  being  followed,  as  in  Figure  1.  No  attempt  is  made  to  find  the 
centroid  of  the  actual  silhouette;  the  center  of  the  box  in  which  it  appears  is  sufficient.  In 
Turner  [8],  it  is  demonstrated  that  a  simple  autopilot  can  be  constructed  that  uses  the  size  of 
the  bounding  box  as  a  range  estimate,  and  the  center  as  a  direction  guide,  that  successfully 
allows  a  UAV  to  follow  another  conventional  airframe  of  similar  capability.  The  reference  is 
included  as  appendix  B. 
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4  Cooperative  Observation  of  Ground  Targets 


One  of  the  most  likely  uses  of  inexpensive  UAVs  is  observation  and  reconnaissance.  In 
this  effort,  we  focused  on  a  particular  problem  that  will  likely  require  cooperation  between 
multiple  vehicles,  that  of  continuous  observation  of  a  partially  occluded  ground  target.  While 
there  has  been  much  work  done  in  the  past  two  decades  on  observation  of  targets  both  by  a 
single  UAV  ([9,  10]  and  others)  and  cooperative  pairs  ([11,  12]  and  many  others),  these  have 
involved  ground  targets  that  are  visible  from  all  angles. 


Figure  2:  Ground  target  partially  hidden  by  obstruction. 

In  this  work,  we  instead  consider  targets  that  are  partially  occluded;  that  is,  they  cannot  be 
seen  from  some  angles,  as  in  Figure  2.  In  the  figure,  there  is  a  single  obstruction,  possibly  a 
building.  The  plane  is  therefore  separated  into  a  “visible”  region,  from  which  the  target  can 
be  seen,  and  the  invisible  region.  The  research  undertaken  assumed  such  a  division,  though 
it  was  not  assumed  that  the  visible  region  was  the  larger. 

The  research  progressed  in  two  steps.  The  first  involved  finding  an  orbit  for  a  single  UAV 
flying  at  constant  altitude  and  velocity  that  stayed  in  the  visible  region.  By  piecing  together 
a  series  of  coordinated  turns,  and  assuming  that  the  vehicle  could  see  slightly  more  than  90 
degrees  from  the  nose,  such  a  trajectory  was  created.  The  performance  index  taken  was  the 
time  of  the  orbit;  the  objective  was  to  maximize  the  time  that  a  single  vehicle  could  maintain 
observation,  so  that  other  UAVs  could  be  used  for  other  tasks.  Descriptive  parameters  were 
varied  to  investigate  the  sensitivity  of  the  orbit  duration. 

The  second  step  was  to  extend  the  methods  of  Barth[ll]  to  this  situation.  It  was  found  that 
the  Lyapunov  control  approach  produced  viable  controller  for  this  situation.  Barth  used  the 
distances  of  the  vehicles  from  the  target,  and  from  each  other,  to  derive  his  controller.  It 
was  shown  in  the  work  reported  here  that  only  one  vehicle  needs  to  be  able  to  see  the  target. 
Full  investigation  of  the  controller  with  this  reduced  information  was  not  completed. 

The  full  report  of  this  effort  is  provided  in  [13],  which  is  enclosed  as  Appendix  C. 
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5  Analytic  Redundancy  and  On-line  Parameter  Esti¬ 
mation 

Two  efforts  were  made  in  this  area.  The  first  involved  neural  net  augmentation  of  a  linear 
autopilot.  The  second  used  standard  parameter  estimation  techniques  to  estimate  during 
flight  the  characteristics  of  the  UAV. 

Recall  that  the  standard  linear  Kalman  Filter  in  discrete  time  has  the  form 


(1) 

(2) 


xjfc+i  =  Akxk  +  Bkuk 
xk  =  xk  +  K  [zk  -  Hkxk\ 

where  the  linear  system  is  defined  by  the  relations 


(3) 

(4) 


xk+i  —  Akxk  +  Bkuk  +  wk 
Zk  =  Hkxk  +  vk. 


In  this  formulation,  xk  G  3ft71  is  the  state,  uk  G  is  the  known  control,  and  zk  G  3ip  is  the 
measurement.  The  process  and  measurements  are  corrupted  by  wk  and  vk,  assumed  to  be 
normally  distributed  and  zero  mean  (any  known  non- zero  mean  is  easily  subtracted  out).  K 
is  the  estimator  update  gain,  the  equation  for  which  can  be  found  in  any  standard  reference. 

Most  importantly  in  the  equations  above,  the  plant,  control,  and  measurement  matrices  A, 
B ,  and  H  are  assumed  known  in  advance  for  all  time  steps.  This  is  true  for  the  data  fusion 
filter,  as  the  dynamics  consist  of  integrating  the  accelerometer  measurements,  and  the  IMU 
measurement  noise  enters  as  w.  The  measurements  come  from  the  GPS,  and  the  system  is 
well-known  to  be  ( A ,  H )  unobservable. 

Were  the  vehicle  dynamics  reliably  known,  it  would  make  more  sense  to  use  them  in  the 
filter.  This  is  made  difficult  for  two  reasons.  One  is  that  the  linear  model  is  only  valid  near 
the  nominal  (usually  straight  and  level)  flight  condition,  while  UAVs  often  take  quite  severe 
maneuvers.  The  other  is  that  inexpensive  UAVs  may  have  very  different  flight  characteristics, 
even  when  they  are  theoretically  identical.  They  also  may  suffer  minor  damage  in  the  field. 

In  this  work,  two  approaches  were  taken  to  dealing  with  this.  The  first  simply  augments 
the  nominal  linear  controller,  as  in  Sharma  and  Calise[14].  This  forces  the  UAV  to  act  as 
the  nominal  UAV  would,  which  is  important  when  the  vehicle  is  operating  in  tight  spaces  or 
near  another  vehicle.  While  useful,  this  approach  is  limited. 

Ideally,  we  would  like  to  be  able  to  operate  not  only  in  tight  confines,  but  when  GPS  is  denied. 
This  is  likely  to  be  increasingly  important.  Thus,  the  second  approach  taken  attempts  to 
derive  the  values  of  the  dynamic  coefficient  matrices  on-line,  taking  advantage  of  the  wealth 
of  information  provided  by  GPS  and  the  vision  sensor.  Then,  when  GPS  fails  or  is  denied, 
the  dynamics  can  be  used  in  a  traditional  filter  to  maintain  a  high-quality  estimate  of  the 
state. 
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5.1  Neural  Net  Augmentation 


In  deriving  the  controller  for  a  UAV,  the  best  that  can  be  done  is  to  use  the  nominal  system 
dynamics  in  the  derivation.  This  creates  a  controller  that  will  probably  work  fairly  well, 
however,  it  is  certain  that  there  will  be  at  least  some  difference  between  the  nominal  system 
and  that  of  any  actual  UAV. 

In  controlling  a  UAV  in  close  quarters  or  in  formation,  it  is  likely  that  we  desire  not  only  a 
stable  controller,  but  one  that  produces  the  actual  commanded  output,  at  lease  for  a  subset 
of  the  states.  Sharma  and  Calise[14]  derived  the  theory  to  allow  a  neural  net  to  augment 
the  nominal  linear  controller  to  achieve  this.  There  are  obvious  restrictions,  but  not  onerous 
ones. 

In  their  work,  however,  the  example  system  was  very  simple.  In  this  effort,  Albs-Bohorquez[15] 
extended  this  work  to  the  longitudinal  dynamics  of  an  aircraft.  In  simulation,  it  was  possible 
to  force  an  aircraft  (a  Cessna  172)  to  follow  nearly  precisely  the  altitude  profile  commanded 
by  a  linear  controller  derived  for  an  entirely  different  craft  (a  NAVION).  This  reference  is 
included  as  Appendix  D. 


5.2  On-line  Parameter  Estimation 

In  this  subset  of  the  effort,  the  UAV  runs  three  filters  in  parallel.  The  first  is  the  enhanced 
data' fusion  filter,  including  the  vision  information  as  in  [6].  The  second  is  a  real-time  param¬ 
eter  estimator,  using  standard  techniques  to  find  values  for  the  aircraft  linear  coefficients. 
The  third  is  a  standard  linear  Kalman  Filter,  which  uses  the  linear  models  from  the  second 
in  a  state  estimator.  This  state  estimator  does  not  use  the  GPS  as  a  sensor.  However,  once 
a  reasonably  accurate  model  of  the  UAV  dynamics  has  been  obtained,  the  Kalman  Filter  is 
able  to  estimate  the  states  with  good  accuracy  based  on  input  from  the  inertial  navigation 
system  and  the  vision  sensor. 

While  the  basic  ideas  of  parameter  estimation  for  a  dynamical  system  are  well-known  [16], 
it  was  not  possible  to  find  an  instance  of  on-line  implementation  for  a  UAV  in  the  current 
literature  at  the  beginning  of  this  effort.  This  is  no  longer  true;  results  that  partly  parallel 
some  of  the  developments  under  this  effort  have  since  appeared  in  Farrell  and  Polycarpou 
[17].  These  investigators  used  the  estimates  in  a  feedback  loop  to  update  ongoing  controls, 
however;  to  our  knowledge  there  has  been  no  other  effort  to  use  the  estimates  in  this  fashion. 

In  this  work,  using  simulated  data,  it  was  possible  to  estimate  the  linear  coefficients  during 
the  first  phase  of  flight.  After  this,  knowledge  of  GPS  was  switched  off,  and  the  vehicle 
continued  using  the  estimated  plant  and  control  coefficient  matrices.  No  knowledge  of  these 
matrices  was  assumed  at  the  beginning  of  the  flight.  Heuristic  methods  were  used  to  deter¬ 
mine  when  the  linear  models  were  sufficiently  good. 

The  results  of  this  effort  were  promising.  The  most  immediate  shortcoming  is  that  the 
models  used  were  the  standard,  separated  longitudinal  and  lateral  linear  dynamics,  as  in 
Nelson[18].  While  this  is  standard,  and  works  very  well  for  flight  near  the  straight  and  level 
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reference  condition,  it  does  not  allow  for  kinds  of  maneuvers  we  expect  from  UAV s  in  tight 
confines.  It  leads  to  a  secondary  problem  in  that  data  during  violent  maneuvers  tends  to 
cause  the  estimates  of  linear  coefficients  to  decay. 

There  are  several  directions  in  which  this  part  of  the  work,  in  particular,  could  be  extended. 
Inclusion  of  some  limited,  well-understood  nonlinear  dynamics  should  be  included  in  the 
dynamics  model.  Weighting  of  the  output  of  the  data  fusion  filter  should  be  included  to 
speed  convergence  of  the  dynamics  estimator.  The  results  of  the  effort,  and  several  further 
suggestions,  are  in  Sviridove[19],  which  is  included  as  Appendix  E. 


6  Conclusion 

The  effort  reported  included  several  subsections,  each  of  which  achieved  at  least  minor  suc¬ 
cess.  Results  are  reported,  primarily  in  appendices.  At  least  two  areas,  those  of  cooperative 
observation  of  occluded  ground  targets,  and  on-line  parameter  estimation,  deserve  much 
more  attention. 

The  effort  reported  has  resulted  in  the  publication  of  one  paper,  and  five  master’s  theses. 
The  theses  are  included  as  appendices,  as  they  are  not  readily  available  without  charge,  and 
because  summary  of  the  significant  results  is  almost  impossible  in  a  several-page  executive 
summary.  The  paper  is  included  because  it  is  not  very  long. 
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Autonomous,  unpiloted  aerial  vehicles  are  considered  an  important  part  of  near-term 
reconnaissance  and  other  missions.  This  paper  considers  the  use  of  such  vehicles  in  which 
GPS  and  inertial  systems  are  the  primary  navigations  sensors,  with  a  vision  sensor  also 
available.  The  vision  sensor  is  used  to  approximate  a  velocity  direction,  and  this  measure¬ 
ment  is  added  to  the  usual  GPS/INS  fusion  filter  to  aid  in  the  orientation  approximation. 
The  gain  in  accuracy  from  this  addition  is  considered.  Farther,  the  use  of  this  measure¬ 
ment  in  cases  in  which  there  are  too  few  satellites  for  a  GPS  position  and  velocity  solution 
is  considered.  When  three  satellites  are  available,  the  range  and  range-rate  information 
combined  with  the  velocity  direction  provides  a  reliable  solution. 


I.  Introduction 

Unmanned  systems  require  information  about  the  vehicle  states  such  as  position,  orientation,  and  so 
forth  to  perform  adequately.  This  information  is  derived  from  the  output  of  sensors  such  as  Global  Positioning 
System  (GPS)  receivers,  and  an  Inertial  Navigation  System  (INS).  In  many  cases,  particularly  for  low- 
cost,  expendable  vehicles,  these  are  off  the  shelf  sensors,  readily  available  in  the  market,  with  poor  signal 
characteristics.  Of  course,  many  of  the  state  values  cannot  be  measured  directly  due  to  physical  constraints. 
In  the  case  examined,  the  available  sensors  are  an  INS,  a  GPS,  and  a  vision  sensor.  We  use  an  Extended 
Kalman  Filter  implementation  of  the  GPS/INS  fusion  filter  here  to  give  us  vehicle  state  estimates  of  sufficient 
accuracy 

Data  from  a  simulated  aircraft  and  sensor  model  was  used  for  the  verification  of  the  Extended  Kalman 
Filter. 


I. A.  Basic  Form  and  Simplify  ing  Assumptions 

I.A.l.  Instrumentation 

The  instrumentation  available  on  the  UAV  includes  a  3-axis  accelerometer,  3-axis  gyro,  and  a  GPS  receiver. 
We  also  have  a  vision  sensor  fixed  to  the  body  coordinate  frame  that  gives  us  velocity  direction  information. 
The  information  from  the  INS  (consisting  of  the  accelerometers  and  the  rate  gyros,  with  some  smoothing 
and  signal  processing)  can  be  read  at  a  much  higher  rate  than  GPS. 

Accelerometers:  The  accelerometers  return  sensed  acceleration  at  their  location,  in  the  accelerometer 
coordinate  frame.  This  is  fixed  to  the  body  frame  and  nominally  aligned  with  it. 

Rate  Gyros:  These  are  co-mounted  with  the  accelerometers,  and  share  an  axis  system.  They  return 
sensed  angular  rates  at  their  location. 
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GPS:  The  GPS  receiver  provides  both  inertial  position  and  inertial  velocity.  The  position  is  provided 
as  latitude,  longitude  and  altitude  above  sea  level.  The  velocity  is  provided  in  Northward,  Eastward  and 
Downward  components. 


I.A.2.  Assumptions 

In  implementing  the  navigation  solution,  we  make  some  simplifying  assumptions.  Some  of  these  are  due  to 
the  quality  of  the  instrumentation  that  we  have,  and  some  due  to  the  nature  of  the  mission  for  which  the 
flight  computer  is  needed. 

Assumption  1.1:  Given  the  level  of  accuracy  of  out  inertial  sensors,  the  rotation  rate  of  the  earth  is 
lost  in  the  noise.  Therefore,  we  will  assume  the  Earth  Centered  Earth  Fixed  (ECEF)  frame  to  be  inertially 
fixed.  Also  the  range  of  operation  of  the  vehicle  is  sufficiently  small  that  the  curvature  of  the  Earth  is 
negligible,  and  we  will  assume  a  flat-Earth  approximation  to  be  sufficient. 

Assumption  1.2:  We  assume  that  the  drift  rate  of  the  accelerometers  and  the  rate  gyros  is  sufficiently 
small  that  it  need  not  be  included  in  modelling  them.  The  estimate  of  the  bias  which  we  assume  to  be 
constant  in  this  case,  should  track  a  small  drift  rate. 

Assumption  1.3:  The  inertial  measurement  unit  is  mounted  sufficiently  close  to  the  center  of  mass  of 
the  vehicle  that  we  need  not  include  the  lever  arm  between  it  and  the  center  of  mass  in  our  calculations. 
Alternatively,  as  in  this  case,  we  can  directly  track  the  position  of  the  IMU  and  base  all  the  relevant 
calculations  on  that. 

Assumption  1.4:  The  position  of  the  GPS  antenna  relative  to  the  center  of  mass  is  non-trivial,  but 
known. 


I.B.  Filter  Structure: 

This  filter  will  differ  from  a  textbook  estimation  filter  in  that  the  “process'5  is  the  state  of  the  system  as 
driven  by  the  readings  from  the  IMU.  Therefore,  the  noise  in  the  IMU  measurements  takes  the  place  of 
process  noise.  It  is  only  the  noise  in  the  GPS  readings  that  comes  into  the  filter  as  measurement  noise.  This 
formulation  of  the  GPS/INS  filter  is  standard,  and  further  details  are  available  in,1  among  others. 

I.C.  Filter  States: 

In  the  case  of  this  particular  navigation  filter  implementation,  we  add  the  decision  to  describe  the  aircraft 
orientation  using  quaternions,  rather  than  the  more  familiar  Euler  angles.  With  this  decision,  we  have  a 
sixteen-state  filter: 


P 

v 


q 

b 

<p 


Inertial  position 
Inertial  velocity 
Quaternions 
Accelerometer  biases 
Rate  Gyro  biases 


A  bias  is  defined  as  a  constant  error  in  the  measurements.  Note  that  we  consider  the  ECEF  frame  to 
be  the  inertial  frame.  Also  in  this  filter  implementation  the  gravitational  effects  are  taken  care  of  by  the 
constant  vector  g  incorporated  in  the  dynamical  model  of  the  system  where  it  is  defined  as  shown  below 
with  units  of  (m/sec2). 

g  =  [o  0  9.8l]  (!) 
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II.  Equations 


II.A.  Measurement  Equations 

Let  as  and  be  the  measured  accelerations  and  angular  velocities,  respectively.  We  have  assumed  that 
these  measurements  are  affected  by  constant  biases  and  white  noise  as  given  in  eq.  (2)  and  eq.  (3)  below, 
where  it  is  understood  that  these  values  are  expressed  in  the  body  coordinate  system. 

8ls  —  a  +  b  +  wa  (2) 

us  =  +  ^  +  (3) 

The  unsubscripted  values  are  truth  values;  those  subscripted  with  s  are  the  sensed  values.  The  terms  wa 
and  are  zero-mean  with  noise  processes  of  known  variances;  wa  ~  N( 0,  Va )  and  N(  0,14) 

The  GPS  measurements  of  position  and  velocity  are  likewise  subject  to  measurement  noise,  but  we 
assume  that  they  have  zero  bias.  Thus, 

Pg  =  pG  +  wp 
vG  =  vG  +  Wp 

The  subscript  G  specifies  that  the  measurement  is  made  by  the  GPS  unit. 


(4) 

(5) 


ILB.  Dynamical  System  Equations: 

The  equations  of  motion  for  these  variables  are 


P  =  V 

(6) 

V  =  a1  +  g 
q  =  iftq 

(7) 

(8) 

where  a1  is  the  imposed  acceleration  on  the  craft  which  accounts  for  all  forces  except  gravity.  It  is  expressed 
in  the  inertial  frame.  f2  is  given  by 

0  —cjx  ~UJy 

ljx  0  ujz  —a;, 

LOy  Z  0  CL/"r 


0 


(9) 


Here,  ujx,ujy ,  and  cjz  are  the  components  of  u>,  the  rotation  rate  of  the  vehicle  with  respect  to  the 
inertial  frame  of  reference. 


In  terms  of  the  measured  values,  we  can  write  the  dynamical  system  equations  as 

p  =  v 

V  =  L/6(q) [a*  -  b]  -  L/bwa  +  g 


(10) 
(11) 

where  L}b  is  the  transformation  matrix  from  the  body  to  the  inertial  frame  of  reference  in  terms  of  the 
quaternions. 


The  dynamic  system  equation  for  the  quaternions  is 


q  =  -  ip) q  -  w„)q 


For  the  constant  accelerometer  and  gyro  biases  we  have 

b  =  0 

(p  =  0 


(12) 


(13) 

(14) 
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III.  Extended  Kalman  Filter  Implementation 


For  completeness,  we  give  a  brief  overview  of  our  implementation  of  the  Extended  Kalman  Filter.  The 
full  derivation  follows  the  one  given  in1  closely. 


Consider  the  equations  for  a  nonlinear  system 

x(t)  =  f(x(i),*)+u;(t) 

z  (**)  =  h(x(4),  tk)  +  1/(4) 


(15) 

(16) 


where  we  assume  that  and  is  defined  for  all  x  and  time  t.  u  and  v  are  assumed  to  be  uncorrelated 
white  noise  processes  having  Gaussian  distribution  as  before. 

At  the  outset  the  reference  or  nominal  trajectory  starts  from  the  initial  condition  x(4)  =  Xq  and  satisfies 


the  differential  equation 

x(t)  =  f  (x(*),t) 

Associated  with  this  nominal  trajactory  would  be  the  measurement  equation 

z(4)  =  h(x(4),4) 

Let 

x  =  x  +  Jx 

then  expanding  about  the  nominal  trajectory  and  dropping  higher-order  terms  gives 

£x(t)  «  F[x(t),  t]<5x  +  u>(t) 

where 


(17) 

(18) 


F[x(t),t]  A 


<9f 

<9x 


(19) 

(20) 


x=x(t) 

Similarly  the  measurement  equation  can  also  be  expanded  as 


z(ffc)  =  h(x(4)  +  <5x,tfc)  +  i/(4) 

ah 


h(x(4),  4)  + 


ax 


&x  + 1/(4) 


(21) 


With  the  definition  of  z(4)  given  in  eq.  (18),  the  linearized  dynamic  equation  for  measurement  perturbations 
becomes 

y(4)  =  z(4)  -  z(4)  =  H[x(4),  4]<5x  +  u{tk)  (22) 

where 

(23) 


H[x(4),4]  4  ^ 


X— x(tfc  ) 

The  extended  Kalman  filter  measurement  update  incorporates  the  measurement  z(4)  =  z k  by  means 


of 


Kfc  =  PfcHHHfcPfcH^  +  Rfc]-1 

x^  =  xfc“  +  Kk[zk  -  Hfc (xfc-)] 
p+  =  p-  -  KfcHfcPr 


(24) 

(25) 

(26) 


where  H  is  given  by  eq.  (23).  Here,  R  is  the  covariance  of  the  sensor  readings.  The  estimate  is  propagated 
forward  to  the  next  sample  time  4+i  using  the  equations 


4+1 


fc+i 


ftk  +  1 

=  x++/  f(x+f) 

Jtk 


dt 


tk 


=  ^fcP^k+Qfc 


(27) 

(28) 
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where  <j>  is  the  state  transition  matrix  defined  by 


<t>  =  F  <j>\  where  <j>(tk)  =  I  (29) 

Note  that  we  have,  as  in1,2  and  others,  chosen  to  use  the  states  themselves  as  our  filtered  variables.  A 
second  approach,  often  used  in  extremely  high- accuracy  implementations,  is  to  write  the  filter  in  terms  of 
the  deviations  from  the  nominal  trajectory,3  and  add  these  corrections  to  the  propagated  nominal. 


III.A.  The  Vision  Based  Update 

We  use  the  standard  pin-hole  camera  model,  as  in.4  While  it  is  possible  in  theory  to  derive  full  state 
information  from  vision  alone,4-6  some  of  the  states  are  much  harder  to  estimate  well.  More  success  is  likely 
when  at  least  some  additional  information  is  available.6 


Figure  1.  Pinhole  camera  model 


In  this  work,  we  have  chosen  instead  to  assume  that  only  one  measurement  is  asked  of  the  vision  sensor. 
It  is  easily  shown5  that  direction  of  flight  of  the  vehicle  is  separable  from  other  apparent  motion  of  objects 
in  the  camera  field  of  view.  We  assume  that  this  measurement  is  delivered  to  the  navigation  filter. 

The  velocity  direction  in  the  body  frame  is  described  by  two  angles  as  shown  in  figure  1.  We  can  see 

that 

Ai  =  tan_1(i)  '  (30) 

A2  =  (31) 

Now,  we  can  convert  the  inertial  velocity  into  the  body  as 


=  Lb/(q)(v)/  (32) 


(the  superscripts  I  and  b  denote  the  respective  frames  of  reference).  We  thus  have  the  two  measurements  Ai 
and  A2  in  terms  of  the  states  vr  and  q.  We  use  this  in  equations  (30)  and  (31),  and  taking  the  appropriate 
derivatives  provides  the  measurement  coefficient  matrix  associated  with  the  vision  Update,  as  in  eqn.  (23). 
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III.B.  Propagation  between  GPS  Measurements 

When  there  is  no  GPS  measurement,  but  there  is  an  inertial  measurement,  we  propagate  the  transition 
matrix  0  and  the  a  priori  estimate  x.  Let  the  measurement  be  taken  at  time  tk  and  let  tk+i  —  ifc  +  At  where 


At  is  a  known  increment  .We  approximate  the  solution  to  the  dynamical  system  equations  as 

Pfc+i  =  Pfc  +  vfcA  t  (33) 

Vfc+i  =  vfc  +  [(L/6(qfc)[aSi&  -  Bk])  +  g]At  (34) 

qfc+i  =  Qjfc  +  ^(ws,fc  -  V>fc)qAi  (35) 

bfc+i  =  bfc  (36) 

<Pk+ i  =  &k  (37) 

Here  as  and  u;s  are  the  measured  accelerations  and  rotations  respectively.  Now  the  process  noise  covariance 
matrix  is  propagated  as  shown  below.  The  value  of  <0  is  approximated  as 

Cf>(tk+1,tk)  *I  +  Fk At +  ±F2kAt2  (38) 


The  matrix  F  is  computed  each  time  an  inertial  measurement  is  available. This  matrix  can  be  written 
in  block  form  as 

0 
0 
0 
0 
_0 

where  using  eq.  (10)  -  eq.  (14)  we  have 

P23 
P24 
F33 
F35 

III.C.  Pseudorange  Rate  Update 

The  preceding  development  assumes  a  full  solution  from  the  GPS  receiver.  He  requires  that  a  minimum 
of  four  satellites  be  in  view  however,  which  may  not  always  be  the  case.  Also,  there  are  likely  to  be  other 
reason  why  we  would  wish  to  use  the  pseudoranges  and  rates  directly,  rather  than  the  receiver  solution. 

Let  the  unit  vector,  u  in  the  direction  of  a  satellite  in  inertial  space  (NED  frame)  be 

11  =  UNl  +  UEj  +  UDk 

where  i,  j,  k  are  the  respective  unit  vectors  in  the  NED  frame.  Since  we  can  express  un,ue,  ud  in  terms  of 
the  azimuth  and  elevation  angles  we  have  the  unit  vector  in  the  direction  of  that  satellite  in  terms  of  known 
quantities.  We  then  take  the  dot  product  of  this  unit  vector  with  the  velocity  estimate 

PSr-ate  =  V7  •  U 

Hence  psrate  gives  us  an  estimate  of  the  pseudorange  rate  in  the  direction  of  that  particular  satellite  in 
inertial  space.  This  when  compared  to  the  measured  value  of  the  pseudorange  rate  from  the  GPS  receiver, 
gives  us  the  error  which  drives  the  filter.  Now  taking  the  appropriate  derivatives  provides  the  measurement 
coefficient  matrix  associated  with  the  pseudorange  rate  update,  as  in  eqn.  (23). 


I3  0 
0  F23 


0  0 


0 

P24 


0 


0  F33  0  F35 

0  0  0  0 


0 


=  (L/b(q)(as  -  b)] 
=  -L/b(q) 

=  ^°(ws  -  <P) 


1  _0_ 
2  dip 
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IV.  Simulation  Runs: 


Data  used  to  verify  the  traditional  GPS/INS  fusion  filter  with  the  vision  update  was  simulated  using  a 
dynamic  model  of  an  aircraft  and  sensor  model.  The  IMU  data  comes  in  at  100  Hz,  the  vision  data  comes 
in  at  20  Hz  and  the  GPS  data  comes  in  at  1Hz.  We  have  a  state  estimate  and  its  associated  error  variance 
coming  out  of  the  filter  at  100  Hz. 

Similarly  while  verifying  the  filter  based  on  the  pseudorange  rate  update  we  bring  in  the  pseudorange 
rate  information  at  1Hz.  The  IMU  and  the  vision  information  comes  in  at  the  same  rate  as  mentioned  in 
the  paragraph  above. 

IV.A.  Choice  of  the  Sensor  Specifications: 

The  covariances  for  the  GPS  have  been  chosen  from  a  variety  of  sources  to  represent  the  typical  values 
for  such  instruments.  The  covariances  used  in  the  INS  were  based  on  the  specifications  provided  in  the 
reference.7 

Process  Noise  Covariance  Q:  Q  is  taken  to  be  a  diagonal  matrix  with  all  entries  zero  other  than  those 
associated  with  the  accelerometers  and  gyros.  The  accelerometer  covariance  is  taken  to  be  0.0144  m/s2  in 
all  channels.  The  gyro  noise  covariance  is  assumed  to  be  0.000144  1/s. 

GPS  Noise  Covariance:  The  diagonal  elements  (variances)  of  the  sensor  noise  covariance  matrix  used 
here  are 

[4.0,4.0,49.0,  0.01,0.01,0.01] 

with  units  of  meters  and  meters  per  second,  as  appropriate. 

Vision  Sensor  Noise  Covariance:  The  diagonal  elements  of  the  vision  noise  covariance  matrix  used  are 

[0.0049,  0.0049]  radians. 

Pseudorange  Rate  Noise  Covariance:  While  verifying  the  filter  based  on  the  pseudorange  rate  update  the 
diagonal  elements  (variances)  of  the  sensor  noise  covariance  matrix  used  are 

[0.1,  0.1,  0.1]  m/s. 


IV.B.  Results: 


IV.B.l.  Filter  with  GPS  and  Vision  Update. 


The  plot  in  figure  2  represents  the  three  dimensional  position  history  of  the  truth  that  we  are  tracking.  The 
aircraft  banks  to  about  ninety  degrees  when  taking  the  two  sharp  left  turns,  one  when  the  y  coordinate  is 
about  —500  m  and  the  next  when  the  y  coordinate  is  about  —4000  m.  (A  third  turn  appears  near  the  end 
of  the  data  set  used.)  Since  the  data  to  be  processed  is  simulated  inflight  data,  the  first  data  set  is  taken  to 
be  the  initial  conditions  for  the  sensors. 


Shown  below  are  the  time  histories  of 
a  couple  of  the  filter  states  along  with 
their  associated  error  variance.  These  plots 
are  based  on  the  data  processed  by  the 
GPS/INS  fusion  filter  with  vision  update. 
In  these  plots,  the  dotted  lines  show  the  his¬ 
tory  without  the  vision  update  whereas  the 
solid  line  shows  the  history  with  the  vision 
update. 


Figure  2.  Three  dimensional  motion 
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IV. C.  Lateral  Position 


Time  (secs) 

(a)  Lateral  position 


Time  (secs) 

(b)  Variance  estimate 


Figure  3.  Lateral  position  estimate  and  estimated  variance. 


Figure  3(a)  and  shows  the  estimated  lateral  position.  The  estimates  with  the  vision  update  and  the  ones 
without  the  update  virtually  superpose.  The  left  turns  taken  by  the  craft  occur  at  around  t  =  125  sec  and 
around  t  =  190  sec.  In  figure  3(b)  notice  the  fact  that  the  excursion  in  the  error  variance  history  without 
the  vision  update  of  about  90m  at  t  —  125  sec  and  50m  at  t  =  190  sec  are  not  seen  in  the  error  variance 
with  the  vision  update.  Similar  behavior  was  seen  in  the  other  position  variables. 

IV. D.  Quaternion-3 


Time  (secs) 

(a)  Quaternion-3 


Time  (secs) 
(b)  Error-q3 


Figure  4.  Estimate  and  variance  of  quaternion  <73 


The  quaternions  describe  the  orientation  of  the  aircraft  in  the  local  frame.  Here  we  see  distinct  differences 
between  the  plots  for  the  estimates  with  the  vision  update  and  the  one  without  it.  The  estimate  is  nearly 
exact  during  the  steady  portion  of  the  flight,  reflecting  the  additional  information  from  the  vision  sensor. 

In  figure  4(b)  we  have  the  estimated  error  variance  for  the  third  quaternion.  Unsurprisingly,  the  variance 
grows  during  the  steady  phases  of  the  flight  when  there  is  no  vision  update.  The  additional  information 
from  the  vision  sensor  makes  the  angular  states  fully  observable,  however,  and  the  variance  estimate  remains 
nearly  zero  when  it  is  included. 
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IV.E.  Filter  with  Pseudorange  Rate  and  Vision  Update. 

In  the  situation  when  we  don’t  have  a  GPS  solution  and  are  unable  to  provide  the  GPS  update  to  the 
filter,  we  can  use  pseudorange  rate  information  from  the  GPS  receiver  to  update  the  filter  until  we  have 
full  GPS  solution  again.  Investigation  in  that  direction  yields  (as  expected)  that  we  need  pseudorange  rate 
information  from  a  minimum  of  three  satellites  to  arrive  at  a  navigation  solution  of  useful  accuracy. 


CO 
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ro 
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time  (secs)  time  (secs) 

(a)  Quaternion-3  (3  satellites)  (b)  Error-q_3  (3  satellites) 

Figure  5.  Estimate  of  qz  using  pseudorange  from  3  satellites. 


As  an  illustration,  we  give  here  the  results  for  the  third  quaternion.  In  this  case,  we  have  used  only 
pseudorange  rate,  without  including  pseudorange,  as  the  immediate  goal  of  our  work  is  primarily  concerned 
with  orientation  angles.  Figure  5(a)  shows  the  estimate  with  3  satellites  in  view.  Comparison  with  Figure 
4(a)  shows  excellent  agreement  with  earlier  results,  and  in  fact  the  estimate  is  close  to  truth.  The  estimated 
variance  is  however  much  smaller. 


Figure  6.  Estimate  of  q3  using  pseudorange  from  2  satellites. 


Just  to  illustrate  what  happens  when  we  have  only  two  satellites  in  view,  figure  6(a)  illustrates  the  same 
estimate  in  such  a  situation  and  figure  6(b)  illustrates  the  associated  error  variance  estimate.  The  estimate 
is  not  as  good,  especially  during  the  steady  cruise  phase  at  the  beginning  of  the  trajectory.  It  dials  in  quite 
well  during  the  later  maneuvering  stages,  however.  The  estimated  variance  indicates  less  confidence  in  the 
estimate.  Formal  observability  analysis  has  not  yet  been  done  in  these  cases. 
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V.  Conclusion 


In  this  paper  the  traditional  GPS/INS  fusion  filter  was  successfully  extended  to  include  a  velocity 
direction  update  from  a  vision  sensor,  and  the  performance  of  the  augmented  filter  was  investigated.  Results 
show  that  the  vision  information  successfully  improves  the  estimates  of  the  state  variables  and  lowers  the 
associated  error  covariances.  Further  work  in  this  direction  using  pseudorange  rate  data  update  instead  of 
the  traditional  GPS  update  shows  us  that  the  filter  performs  satisfactorily  with  pseudorange  rate  data  from 
a  minimum  of  three  satellites,  and  surprisingly  well  with  data  from  only  two.  This  could  potentially  lead  to 
the  filter  being  robust  to  the  loss  of  GPS,  at  least  for  a  short  period  of  time.  The  results  also  indicate  that 
the  direct  use  of  pseudorange  rate  information  (as  opposed  to  a  velocity  solution  from  the  GPS  receiver) 
might  be  advisable  for  the  estimation  of  orientation  angles. 

Acknowledgment 

This  work  was  partly  supported  by  the  Air  Force  Office  of  Scientific  Research  through  award  no.  FA9550- 
05-1-0296. 


References 

1  Farrell,  Jay,  A.,  and  Matthew  Barth,  The  Global  Positioning  System  &  Inertial  Navigation ,  McGraw  Hill  New  York, 

1999. 

2Dittrich,  Joerg,  S.,  Design  and  Integration  of  an  Unmanned  Aerial  Vehicle  Navigation  System ,  Masters  Thesis,  School 
of  Aerospace  Engineering,  Georgia  Institute  of  Technology,  Atlanta,  Georgia  2002. 

3Williamson,  W.  R.,  M.  F.  Abdel-Hafez,  I.  Rhee,  E.  J.  Song,  J  D.  Wolfe,  D.  F.  Chichka,  and  J.  L.  Speyer,  “An 
Instrumentation  System  Applied  to  Formation  Flight” ,  IEEE  Transactions  on  Control  System  Technology ,  to  appear. 

4Ma.  Yi,  Stafano  Soatto,  Jana  Kosecka,  and  S.  Shankar  Sastry,  An  Invitation  to  3-D  Vision ,  series  Interdisciplinary 
Applied  Mathematics,  Springer- Verlag,  New  York,  NY,  2004. 

5Chakravarty,  Rajtilok,  Adding  Vision  Information  to  a  GPS/INS  Fusion  Filter ,  Master’s  Thesis,  School  of  Engineering 
and  Applied  Science,  The  George  Washington  University,  Washington,  DC,  2006. 

6Iyer,  R.  V.,  Zhihai  He,  and  Philip  R.  Chandler.  “On  the  Computation  of  the  Ego-Motion  and  Distance  to  Obstacles  for 
a  Micro  Air  Vehicle”  Proceedings  of  the  IEEE  American  Control  Conference ,  to  appear. 

7Crista  Inertial  Measurement  Unit  Interface  /  Operation  Document ,  Cloud  Cap  Technology,  Hood  River,  OR,  May  2004. 


10  of  10 


American  Institute  of  Aeronautics  and  Astronautics 


